#include "imu.h"
#include "mpu6050.h"
#include "Butterworth_filter.h"
#include "vector3.h"
#include "delay.h"
#include "filter.h"

 
Vector3f_t acc_off;
extern FLOAT_XYZ GYRO_OFFSET_RAW,ACC_OFFSET,ACC_SCALE;	
extern INT16_XYZ MPU6050_BodyACC_RAW,MPU6050_BodyGYRO_RAW;	 

Vector3f_t Acc_BodyNavigation,ACC_w;
FLOAT_XYZ Gyro_Raw,Gyro_20hz,Acc_15HZ,Acc_filt,Gyr_rad,Acc_Butter,Gyr_Butter;

float ACCZ_M=0.0f;
/******************************************************************************
*函  数：void IMU_Data_PLUS(void)
*功  能：IMU数据处理
*******************************************************************************/
void IMU_Data_PLUS(void)  
{
	
	static FLOAT_XYZ Acc;
	static FLOAT_XYZ Gyro;
	
    MPU6050_Read();           //从X轴加速度寄存器开始读，14个数据，读进入BUFF 
	
  //陀螺仪转换成弧度/秒      
	Gyro.X = (float) MPU6050_BodyGYRO_RAW.X * Gyro_Gr - GYRO_OFFSET_RAW.X;  
	Gyro.Y = (float) MPU6050_BodyGYRO_RAW.Y * Gyro_Gr - GYRO_OFFSET_RAW.Y;
	Gyro.Z = (float) MPU6050_BodyGYRO_RAW.Z * Gyro_Gr - GYRO_OFFSET_RAW.Z;
	
	 //加速度变成g再转换成ms2/s
    Acc.X = (float)MPU6050_BodyACC_RAW.X * Acc_Gain * G * ACC_SCALE.X - ACC_OFFSET.X;  
	Acc.Y = (float)MPU6050_BodyACC_RAW.Y * Acc_Gain * G * ACC_SCALE.Y - ACC_OFFSET.Y;   
	Acc.Z = (float)MPU6050_BodyACC_RAW.Z * Acc_Gain * G * ACC_SCALE.Z - ACC_OFFSET.Z;	
		
//	Acc_Butter = ACC_20HZLP_Filter(&Acc);
//	Gyr_Butter = GYRO_30HZLP_Filter(&Gyro);
	
//	Acc_filt = ACC_20HZLP_Filter(&Acc);
//	Gyr_rad = GYRO_30HZLP_Filter(&Gyro);

	
//	Acc_filt = LP100_20HZ_Filter(&Acc); 
//	Gyr_rad = LP100GYRO_20HZ_Filter(&Gyro); 
	
	Acc_filt.X = Acc.X;
	Acc_filt.Y = Acc.Y;
    Acc_filt.Z = Acc.Z;
	
	Gyr_rad.X = Gyro.X;
	Gyr_rad.Y = Gyro.Y;
    Gyr_rad.Z = Gyro.Z;
	
	

//    RC_Filter_(0.05,Acc.X,&Acc_filt.X);  //这个滤波初值一定要赋值好      0.5
//	RC_Filter_(0.05,Acc.Y,&Acc_filt.Y);  //这个滤波初值一定要赋值好
//	RC_Filter_(0.05,Acc.Z,&Acc_filt.Z);  //这个滤波初值一定要赋值好，


//	Acc_15HZ = ACC_15HZLP_Filter(&Acc);
//	Gyro_20hz = GYRO_20HZLP_Filter(&Gyro);

//    RC_Filter_(0.05,Acc.X,&Acc_BodyNavigation.x);  //这个滤波初值一定要赋值好      0.5
//	RC_Filter_(0.05,Acc.Y,&Acc_BodyNavigation.y);  //这个滤波初值一定要赋值好
//	RC_Filter_(0.05,Acc.Z,&KL);  //这个滤波初值一定要赋值好，
	

 	Acc_BodyNavigation.x = Acc_filt.X - acc_off.x;
    Acc_BodyNavigation.y = Acc_filt.Y - acc_off.y;	
	Acc_BodyNavigation.z = Acc_filt.Z - acc_off.z;	
		
//	RC_Filter_(0.95,Acc_BodyNavigation.x,&ACC_w.x); 
//	RC_Filter_(0.95,Acc_BodyNavigation.y,&ACC_w.y);  
//	RC_Filter_(0.95,Acc_BodyNavigation.z,&ACC_w.z); 
	
	RC_Filter_(0.9,Acc_BodyNavigation.x,&ACC_w.x); 
	RC_Filter_(0.9,Acc_BodyNavigation.y,&ACC_w.y);  
	RC_Filter_(0.001f,Acc_BodyNavigation.z,&ACC_w.z); 
	
	
	Gyro_Raw.X = Gyro.X;
	Gyro_Raw.Y = Gyro.Y;
	Gyro_Raw.Z = Gyro.Z;
	
	
}

/******************************************************************************
*函  数：void IMU_Data_ACMP(void)
*功  能：求Z运动加速度零偏
*******************************************************************************/
void IMU_Navigation_ACMP(void)  //求Z运动加速度零偏
{	
	 static FLOAT_XYZ Acco;
	 static float acc_zoff = 0;
	 static float acc_yoff = 0;
	 static float acc_xoff = 0;
	 for(uint8_t i=0;i<200;i++)
	 {
		 MPU6050_Read();  
		 Acco.Z = (float)MPU6050_BodyACC_RAW.Z * Acc_Gain * G * ACC_SCALE.Z - ACC_OFFSET.Z;	
		 Acco.X = (float)MPU6050_BodyACC_RAW.X * Acc_Gain * G * ACC_SCALE.X - ACC_OFFSET.X;	
		 Acco.Y = (float)MPU6050_BodyACC_RAW.Y * Acc_Gain * G * ACC_SCALE.Y - ACC_OFFSET.Y;	
	     Acc_filt = ACC_20HZLP_Filter(&Acco);
	
	     ACCZ_M = Acc_filt.Z - G;       //z轴的运动加速度
		 
		 acc_zoff += ACCZ_M; 
		 acc_yoff += Acc_filt.Y;
		 acc_xoff += Acc_filt.X;
		 delay_ms(5);
	 }
	 acc_off.x = acc_xoff/200;
	 acc_off.y = acc_yoff/200;
	 acc_off.z = acc_zoff/200;
}




